#include "Controller.h"

Controller::Controller()
{
	HAL::InitProject();
	this->OUTPUTS.VehicleSpeedOut	= OutSignal (OUT_VEH_SPEED_RAW_MAX,		OUT_VEH_SPEED_RAW_MIN,		OUT_VEH_SPEED_PHYS_MAX,		OUT_VEH_SPEED_PHYS_MIN);
	this->OUTPUTS.EngineSpeed		= OutSignal (OUT_ENGINE_SPEED_RAW_MAX,	OUT_ENGINE_SPEED_RAW_MIN,	OUT_ENGINE_SPEED_PHYS_MAX,	OUT_ENGINE_SPEED_PHYS_MIN);
	this->OUTPUTS.FuelTanLevel		= OutSignal (OUT_FUEL_LVL_RAW_MAX,		OUT_FUEL_LVL_RAW_MIN,		OUT_FUEL_LVL_PHYS_MAX,		OUT_FUEL_LVL_PHYS_MIN);

	this->INPUTS.AccelIn = InSignal(	IN_ACCEL_RAW_MAX,	IN_ACCEL_RAW_MIN,	IN_ACCEL_PHYS_MAX,	IN_ACCEL_PHYS_MIN, SensorFilterCoeffs);
	this->INPUTS.BrakeIn = InSignal(	IN_BRAKE_RAW_MAX,	IN_BRAKE_RAW_MIN,	IN_BRAKE_PHYS_MAX,	IN_BRAKE_PHYS_MIN, SensorFilterCoeffs);
}

void Controller::Step()
{
	if( HAL::timerCnt >= 1000) // alle 10ms
	{
		HAL::timerCnt = 0;
		// this->ReadInputs();
		// this->car.DRIVE();
		this->SetOutputs();
	}
}

void Controller::ReadInputs(void)
{
	/* read inputs */
	this->INPUTS.AccelIn.setRawValue(HAL::ReadAccelSensor());
	this->INPUTS.BrakeIn.setRawValue(HAL::ReadBrakeSensor());

	/* calc reactions */
	this->car.UpdateSpeedDes(INPUTS.AccelIn.getPhysValue());
	this->car.UpdateBrakeDes(INPUTS.BrakeIn.getPhysValue());
}

void Controller::SetOutputs(void)
{
	/* set outputs */
	this->OUTPUTS.VehicleSpeedOut.setPhysValue(this->car.GetVehicleSpeed());

	UInt16 speedSignalToSet = OUTPUTS.VehicleSpeedOut.getRawValue();
	if(speedSignalToSet < 10)
	{
		speedSignalToSet = 10;
	}

	HAL::SetCarSpeedServo(199 - speedSignalToSet);
	this->OUTPUTS.EngineSpeed.setPhysValue(this->car.GetEngineSpeed());
	HAL::SetEngineServo(199 - OUTPUTS.EngineSpeed.getRawValue());

	// set values for diagnostic
	Diagnostic::vSpeed = this->car.GetVehicleSpeed();
	Diagnostic::vGear  = this->car.GetGear();
	Diagnostic::vFuel  = this->car.GetFuel();
	Diagnostic::vEngineSpeed = this->car.GetEngineSpeed();
}
